#include "damiao.h"
#include "arm_math.h"
#include "drivers/dev_can.h"
#include "rtklibc.h"
#include "rtthread.h"
#include "rttypes.h"

static uint8_t idx;
static rt_slist_t dmm_motor_list= {RT_NULL}; // 会在control任务中遍历该指针数组进行pid计算

#define LOG_TAG              "dm"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>
rt_inline void dmmotor_motor_list_init(rt_slist_t *l)
{
    l->next = RT_NULL;//0
}

rt_inline void dmmotor_motor_list_append(rt_slist_t *l, rt_slist_t *n)
{
    struct rt_slist_node *node;

    node = l;
    while (node->next) node = node->next;

    /* append the node to the tail */
    node->next = n;
    n->next = RT_NULL;
}
rt_inline void list_append(DMMOTOR_t *device)
{
    dmmotor_motor_list_init(&(device->list));
    dmmotor_motor_list_append(&dmm_motor_list, &(device->list));
}


/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

void DMMotorSetMode(DMMotor_Mode_e cmd, DMMOTOR_t *motor)
{
    struct rt_can_msg can_msg={0};
    // rt_memset(can_msg.data,0xff,7); // 发送电机指令的时候前面7bytes都是0xff
    can_msg.data[0]=0xff;
    can_msg.data[1]=0xff;
    can_msg.data[2]=0xff;
    can_msg.data[3]=0xff;
    can_msg.data[4]=0xff;
    can_msg.data[5]=0xff;
    can_msg.data[6]=0xff;
    can_msg.data[7] = (uint8_t)cmd; // 最后一位是命令id
    can_msg.id = motor->CanMessage_Init.tx_id+motor->DMMotor_Mode_type;
    can_msg.ide = RT_CAN_STDID;
    can_msg.rtr =RT_CAN_DTR;
    can_msg.len = 8;
    rt_device_write(*motor->CanMessage_Init.can_handle,0,&can_msg,sizeof(can_msg));
}

/**
************************************************************************
* @brief:      	mit_ctrl: MIT模式下的电机控制函数
* @param[in]:   hcan:			指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id:	电机ID，指定目标电机
* @param[in]:   pos:			位置给定值
* @param[in]:   vel:			速度给定值
* @param[in]:   kp:				位置比例系数
* @param[in]:   kd:				位置微分系数
* @param[in]:   torq:			转矩给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送MIT模式下的控制帧。
************************************************************************
**/
void mit_ctrl(DMMOTOR_t *motor, float pos, float vel,float kp, float kd, float torq)
{
	struct rt_can_msg can_msg;
	uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
	
	pos_tmp = float_to_uint(pos,  DM_P_MIN,  DM_P_MAX,  16);
	vel_tmp = float_to_uint(vel,  DM_V_MIN,  DM_V_MAX,  12);
	kp_tmp  = float_to_uint(kp,   DM_KP_MIN, DM_KP_MAX, 12);
	kd_tmp  = float_to_uint(kd,   DM_KD_MIN, DM_KD_MAX, 12);
	tor_tmp = float_to_uint(torq, DM_T_MIN,  DM_T_MAX,  12);

    can_msg.id = motor->CanMessage_Init.tx_id + MIT_MODE;
    can_msg.ide = RT_CAN_STDID;
    can_msg.rtr =RT_CAN_DTR;
    can_msg.len = 8;
	can_msg.data[0] = (pos_tmp >> 8);
	can_msg.data[1] = pos_tmp;
	can_msg.data[2] = (vel_tmp >> 4);
	can_msg.data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
	can_msg.data[4] = kp_tmp;
	can_msg.data[5] = (kd_tmp >> 4);
	can_msg.data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
	can_msg.data[7] = tor_tmp;
	
	rt_device_write(*motor->CanMessage_Init.can_handle,0,&can_msg,sizeof(can_msg));
}
/**
************************************************************************
* @brief:      	pos_speed_ctrl: 位置速度控制函数
* @param[in]:   hcan:			指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id:	电机ID，指定目标电机
* @param[in]:   vel:			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送位置速度控制命令
************************************************************************
**/
void pos_speed_ctrl(DMMOTOR_t *motor, float pos, float vel)
{
	struct rt_can_msg can_msg;
	uint8_t *pbuf, *vbuf;
	
	can_msg.id = motor->CanMessage_Init.tx_id + POS_MODE;
    can_msg.ide = RT_CAN_STDID;
    can_msg.rtr =RT_CAN_DTR;
    can_msg.len = 8;
	pbuf=(uint8_t*)&pos;
	vbuf=(uint8_t*)&vel;
	
	can_msg.data[0] = *pbuf;
	can_msg.data[1] = *(pbuf+1);
	can_msg.data[2] = *(pbuf+2);
	can_msg.data[3] = *(pbuf+3);
	can_msg.data[4] = *vbuf;
	can_msg.data[5] = *(vbuf+1);
	can_msg.data[6] = *(vbuf+2);
	can_msg.data[7] = *(vbuf+3);
	
	rt_device_write(*motor->CanMessage_Init.can_handle,0,&can_msg,sizeof(can_msg));
}
/**
************************************************************************
* @brief:      	speed_ctrl: 速度控制函数
* @param[in]:   hcan: 		指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   vel: 			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送速度控制命令
************************************************************************
**/
void speed_ctrl(DMMOTOR_t *motor, float vel)
{
	
	uint8_t *vbuf;
	struct rt_can_msg can_msg;
	can_msg.id = motor->CanMessage_Init.tx_id + SPEED_MODE;
    can_msg.ide = RT_CAN_STDID;
    can_msg.rtr =RT_CAN_DTR;
    can_msg.len = 4;
	
	vbuf=(uint8_t*)&vel;
	
	can_msg.data[0] = *vbuf;
	can_msg.data[1] = *(vbuf+1);
	can_msg.data[2] = *(vbuf+2);
	can_msg.data[3] = *(vbuf+3);
	
	rt_device_write(*motor->CanMessage_Init.can_handle,0,&can_msg,sizeof(can_msg));
}

void DMMotorDecode(struct rt_can_msg can_msg)
{
    rt_slist_t *node = RT_NULL;
    DMMOTOR_t *dmdevice = RT_NULL;

    rt_slist_for_each(node, &dmm_motor_list)
    {
        dmdevice = rt_slist_entry(node,DMMOTOR_t,list);
        if (dmdevice == RT_NULL)
        {
            LOG_E("Invalid dmdevice pointer\n");
            continue;
        }
        if (can_msg.id == dmdevice->CanMessage_Init.rx_id)
        {
            offline_event_time_update(dmdevice->offline_manage_dm.event);
            uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量

            dmdevice->measure.last_position = dmdevice->measure.position;
            tmp = (uint16_t)((can_msg.data[1] << 8) | can_msg.data[2]);
            dmdevice->measure.position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);

            tmp = (uint16_t)((can_msg.data[3] << 4) | can_msg.data[4] >> 4);
            dmdevice->measure.velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);

            tmp = (uint16_t)(((can_msg.data[4] & 0x0f) << 8) | can_msg.data[5]);
            dmdevice->measure.torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);

            dmdevice->measure.T_Mos = (float)can_msg.data[6];
            dmdevice->measure.T_Rotor = (float)can_msg.data[7];
        }
    }
}

void DMMotorCaliEncoder(DMMOTOR_t *motor)
{
    DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
    rt_thread_mdelay(10);
}
DMMOTOR_t *DMMotorInit(Motor_Init_Config_s *config,rt_uint32_t DM_Mode_type)
{
    DMMOTOR_t *motor = (DMMOTOR_t *)rt_malloc(sizeof(DMMOTOR_t));
    if (motor == RT_NULL) {
        LOG_E("Failed to allocate memory for DJIMotor\n");
        return RT_NULL;
    }
    rt_memset(motor, 0, sizeof(DMMOTOR_t));
    list_append(motor);

    motor->motor_type = config->motor_type;
    motor->motor_settings = config->controller_setting_init_config;
    motor->CanMessage_Init = config->can_init_config;
    
    // motor controller init 电机控制器初始化
    PIDInit(&motor->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
    PIDInit(&motor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
    PIDInit(&motor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
    motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
    motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
    motor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
    motor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
    
    motor->DMMotor_Mode_type =DM_Mode_type;
    //掉线检测
    motor->offline_manage_dm.event = config->offline_manage_motor.event;
    offline_event_init(config->offline_manage_motor);


    DMMotorSetMode(DM_CMD_CLEAR_ERROR,motor);
    rt_thread_mdelay(10);
    DMMotorEnable(motor);
    rt_thread_mdelay(10);
    return motor;
}

void DMMotorSetRef(DMMOTOR_t *motor, float ref)
{
    motor->motor_controller.pid_ref = ref;
}

void DMMotorEnable(DMMOTOR_t *motor)
{
    motor->stop_flag = MOTOR_ENALBED;
    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
}

void DMMotorStop(DMMOTOR_t *motor)
{
    motor->stop_flag = MOTOR_STOP;
}

void DMMotorOuterLoop(DMMOTOR_t *motor, Closeloop_Type_e type)
{
    motor->motor_settings.outer_loop_type = type;
}


void DMMotorcontrol(void)
{
    DMMOTOR_t *motor =RT_NULL;
    float pid_measure, pid_position,pid_ref;             // 电机PID测量值和设定值

    rt_slist_t *node = RT_NULL;
    rt_slist_for_each(node, &dmm_motor_list)
    {
        motor = rt_slist_entry(node,DMMOTOR_t,list);
        if (motor->offline_manage_dm.online_state == STATE_ONLINE)
        {
            pid_ref = motor->motor_controller.pid_ref;
            if (motor->motor_settings.motor_reverse_flag == MOTOR_DIRECTION_REVERSE) { pid_measure *= -1; }

            // pid_ref会顺次通过被启用的闭环充当数据的载体
            // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出

            switch (motor->DMMotor_Mode_type)
            {
            case MIT_MODE:
                LIMIT_MIN_MAX(pid_measure, DM_T_MIN, DM_T_MAX);
                if (motor->stop_flag == MOTOR_STOP) { DMMotorSetMode(DM_CMD_RESET_MODE, motor); };
                mit_ctrl(motor, 0, 0, 0, 0, pid_measure);
                rt_thread_mdelay(2);
                break;

            case POS_MODE:
                LIMIT_MIN_MAX(pid_measure, DM_P_MIN, DM_P_MAX);
                // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
                if ((motor->motor_settings.close_loop_type & ANGLE_LOOP) && motor->motor_settings.outer_loop_type == ANGLE_LOOP)
                {
                    if (motor->motor_settings.angle_feedback_source == OTHER_FEED)
                        pid_measure = *motor->motor_controller.other_angle_feedback_ptr;
                    else
                        pid_measure = motor->measure.position; // MOTOR_FEED,闭环,防止在边界处出现突跃
                    // 更新pid_ref进入下一个环
                    pid_ref = PIDCalculate(&motor->motor_controller.angle_PID, pid_measure, pid_ref);
                }
                if (motor->stop_flag == MOTOR_STOP) { DMMotorSetMode(DM_CMD_RESET_MODE, motor); };
                pos_speed_ctrl(motor, pid_ref, PI);
                rt_thread_mdelay(2);
                break;
            case SPEED_MODE:
                LIMIT_MIN_MAX(pid_ref, DM_V_MIN, DM_V_MAX);
                if (motor->stop_flag == MOTOR_STOP) { DMMotorSetMode(DM_CMD_RESET_MODE, motor); };
                speed_ctrl(motor, pid_ref);
                rt_thread_mdelay(2);
                break;
            }
        }
        else
        {
            DMMotorSetMode(DM_CMD_RESET_MODE, motor);
        }
    }
}